#include "force.h"

// FORCES //

void GlobalForce::AddParticle(Particle *p) {
	affectedParticles.push_back(p);
}

void GlobalForce::AddParticles(vector<Particle*>& particleList) {
	for (vector<Particle*>::iterator i = particleList.begin(); i != particleList.end(); i++) {
		affectedParticles.push_back((*i));
	}
}

ConstantForce::ConstantForce(Vec3d constForce) : _constForce(constForce){ }

void ConstantForce::accountForces() {
	for (vector<Particle*>::iterator i = affectedParticles.begin(); i != affectedParticles.end(); i++) {
		(*i)->accumulateForce(_constForce);
	}
}

RandomForce::RandomForce(double magnitude, double timeToLive, double step)
:ConstantForce( Vec3d(((double) rand() / (RAND_MAX)) - 0.5, ((double) rand() / (RAND_MAX))- 0.5, ((double) rand() / (RAND_MAX)) - 0.5) ){
	_timeToLive = timeToLive;
	_step = step;
	_constForce.normalize();
	_constForce = magnitude * _constForce;
}

void RandomForce::accountForces() {
	if( _timeToLive > 0 ) {
		for (vector<Particle*>::iterator i = affectedParticles.begin(); i != affectedParticles.end(); i++) {
			(*i)->accumulateForce(_constForce);
		}

		_timeToLive -= _step;
	}
}

void RandomForce::decayForceLife(double step) {
	_timeToLive -= step;
}

ConstantGravitationalField::ConstantGravitationalField(Vec3d gFieldStrength) :_gFieldStrength(gFieldStrength) { }

void ConstantGravitationalField::accountForces() {
	for (vector<Particle*>::iterator i = affectedParticles.begin(); i != affectedParticles.end(); i++) {
		Vec3d forceStrength = (*i)->getMass() * _gFieldStrength;
		(*i)->accumulateForce(forceStrength);
	}
}

DragForce::DragForce(double kd) : _kd(kd){ }

void DragForce::accountForces() {
	for (vector<Particle*>::iterator i = affectedParticles.begin(); i != affectedParticles.end(); i++) {
		(*i)->accumulateForce(-_kd * (*i)->getVelocity());
	}
}

void PairForce::addParticlePair(Particle* p1, Particle* p2) {
	pairs.push_back(ParticlePair(p1, p2));
}

SpringForce::SpringForce(double ks, double kd) : _ks(ks), _kd(kd) { }

void SpringForce::addParticlePair(Particle* p1, Particle* p2) {

	// Add displacement length
	restLengths.push_back((p2->getPosition() - p1->getPosition()).length());
	
	// Call base class
	PairForce::addParticlePair(p1, p2);
}

// Model after hooke's law - copy and paste directly from notes (damped spring)
void SpringForce::accountForces() {

	vector<double>::iterator q = restLengths.begin();
	for (vector<ParticlePair>::iterator i = pairs.begin(); i != pairs.end(); i++) {

		Vec3d diffPos = (*i).first->getPosition() - (*i).second->getPosition();
		Vec3d diffVelocity = (*i).first->getVelocity() - (*i).second->getVelocity();

		// Compute f1
		double directCause = _ks * (diffPos.length() - (*q));
		double dampingCause = _kd * (diffPos * diffVelocity) / (diffPos.length());
		Vec3d direction = diffPos / diffPos.length();
		Vec3d f1 = -(directCause + dampingCause) * direction;
		
		// By newton's law
		Vec3d f2 = -f1;

		(*i).first->accumulateForce(f1);
		(*i).second->accumulateForce(f2);

		q++;
	}
}